#ros lib
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool
#common lib
import math
import numpy as np
import time
from time import sleep
from yahboomcar_laser.common import *
print ("import done")
RAD2DEG = 180 / math.pi

class laserStaticWarning(Node):
    def __init__(self,name):
        super().__init__(name)
        #create a sub
        self.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)
        self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)
        #create a pub
        self.pub_Buzzer = self.create_publisher(Bool,'/Buzzer',1)
        
        #declare param
        self.declare_parameter("LaserAngle",15.0)
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.declare_parameter("ResponseDist",0.55)
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
        self.declare_parameter("Switch",False)
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        
        self.Right_warning = 0
        self.Left_warning = 0
        self.front_warning = 0
        self.Joy_active = False
        self.Buzzer_state = False
        
        self.timer = self.create_timer(0.01,self.on_timer)
        
    def on_timer(self):
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value

    def JoyStateCallback(self, msg):
        if not isinstance(msg, Bool): return
        self.Joy_active = msg.data

    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        minDistList = []
        minDistIDList = []
        
        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.LaserAngle):
                minDistList.append(ranges[i])
                minDistIDList.append(angle)
        
        if len(minDistList) == 0: return
        minDist = min(minDistList)
        minDistID = minDistIDList[minDistList.index(minDist)]
        
        # 如果手柄激活或开关开启，暂停检测
        if self.Joy_active or self.Switch == True:
            if self.Buzzer_state == True:
                self.pub_Buzzer.publish(Bool())
                self.Buzzer_state = False
            return
        
        # 静态报警逻辑 - 只检测距离并控制蜂鸣器
        if minDist <= self.ResponseDist:
            if self.Buzzer_state == False:
                b = Bool()
                b.data = True
                self.pub_Buzzer.publish(b)
                self.Buzzer_state = True
                print(f"警告: 检测到障碍物，距离: {minDist:.2f}m，角度: {minDistID:.2f}°")
        else:
            if self.Buzzer_state == True:
                self.pub_Buzzer.publish(Bool())
                self.Buzzer_state = False
                print("解除警告: 无障碍物")
        
    def shutdown_cleanup(self):
        """退出时的清理工作"""
        print("正在安全退出...")
        
        # 关闭蜂鸣器
        for _ in range(3):
            self.pub_Buzzer.publish(Bool())
            time.sleep(0.05)
        print("已关闭蜂鸣器")

def main():
    rclpy.init()
    laser_warn = laserStaticWarning("laser_static_warning")
    print ("静态雷达报警系统启动")
    try:
        rclpy.spin(laser_warn)
    except KeyboardInterrupt:
        pass
    finally:
        laser_warn.pub_Buzzer.publish(Bool())
        laser_warn.shutdown_cleanup()
        laser_warn.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()